#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <vector>
#include <string>
#include <cmath>
#include <uav/Cmd.h>

class Uav
{
public:

    
	// constructor
    Uav();
    // ~Uav();
    void Setup(const int& id, const ros::NodeHandle& nh, ros::Rate& rate);

    // id of uav instance
    int uavid;

    void home(double x,double y,double z);
    
    // arrival at the targer position or not
    bool arrival=false;

    // vector pointer to store the next target positions 
    std::vector<uav::Cmd> *cmdposes;

    // cache for the target position
    uav::Cmd cmdpose;

    // the pointer of targer position
    geometry_msgs::PoseStamped *pose;
    // the pointer of present local position
    geometry_msgs::PoseStamped *nowpose;
    // the pointer of velocity command
    geometry_msgs::Twist *velocity;
    // the array to store the scan data
    double *scan_ranges;
    double *sonar_range;
    double scan_count=0;


    // array to store the cos/sin of angle of laser point
    double cos_scan[360];
    double sin_scan[360];
    
    // initialization for uav instance	
    void init();

    // send offboard command
    void enable_offboard();

    // send arm command
    void enable_armed();

    // fly to the target position (front of <cmdposes>)
    void fly_2_target();

    // fly to the target position (front of <cmdposes>)
    void send_vel_cmd(double vx, double vy, double vz);

    // print local position 
    void print_local_pose();

    // add some new positions to the front of <cmdposes> reversely
    void add_target_front(int cmd_num, double* x, double* y, double* z);

    // add some new positions to the last of <cmdposes> formally
    void add_target_last(int cmd_num, double* x, double* y, double* z);

    // add APF barrier
    void add_APF_barrier(double x, double y, double z, double coff=4.0);

    // add APF target
    void add_APF_target(double x, double y, double z, double coff=1.0);
    void add_APF_cmdpose();     // use cmdposes vector

    // add APF force from laser data
    void add_APF_scan();

    // send APF-updated velocity and cleanup it
    void send_APF_cmd();

    // publishers
    ros::Publisher local_pos_pub;
    ros::Publisher local_vel_pub;


private:
    double initx=0.0;
    double inity=0.0;
    double initz=0.0;

    // record number for arrival check
    int max_check_number=10;
    int check_number=0;

    // initialization for uav instance
    void wait_for_FCU();        // wait for the connection signal
    void send_setpoints();      // send some points to ensure the offboard mode
    void set_mode();            // set_mode : OFFBOARD
    void set_arm_cmd();         // set_arm  : TRUE

    // callback functions
    void state_cb(const mavros_msgs::State::ConstPtr& msg);
    void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void cmd_cb(const uav::Cmd::ConstPtr& msg);
    void scan_cb(const sensor_msgs::LaserScan::ConstPtr& msg);
    void sonar_cb(const sensor_msgs::Range::ConstPtr& msg);

    // subscribers
    ros::Subscriber position_sub;
    ros::Subscriber state_sub;
    ros::Subscriber cmd_sub;
    ros::Subscriber scan_sub;
    ros::Subscriber sonar_sub;
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;

    // some messages to publish
    mavros_msgs::State current_state;
    mavros_msgs::SetMode offb_set_mode;
    mavros_msgs::CommandBool arm_cmd;

    // nodehandle of instance (deliveried from ROS node)
    ros::NodeHandle nh_;

    // internal rate timer  
    ros::Rate *rate_;

};
